import socket
import sys
import threading

lock = threading.Lock()


def send_command(s: socket.socket, cmd_str: str):
    """
    :param s: 发送消息套接字
    :param cmd_str: 命令
    :return: return_msg
    """
    cmd_str += ";"
    lock.acquire()
    s.send(cmd_str.encode('utf-8'))
    try:
        # 等待机器人返回执行结果
        buf = s.recv(1024)
        lock.release()
    except socket.error as e:
        print("Error receiving :", e)
        lock.release()
        sys.exit(1)
    # 去除返回结果的最后的';'字符
    echo = (buf.decode('utf-8'))[:-1]
    return echo


def command():
    """
    :return: cmd_str
    """
    return "command"


def robot_mode(mode: str):
    """
    :param mode: chassis_lead : 云台跟随底盘模式
                 gimbal_lead : 底盘跟随云台模式
                 free : 自由模式
    :return: cmd_str
    """
    return "robot mode " + mode


def set_sensor_measure(switch: bool):
    """
    :param switch: True:on
                   False:off
    :return: cmd_str
    """
    cmd = "ir_distance_sensor measure "
    if switch:
        cmd += "on"
    else:
        cmd += "off"
    return cmd


def get_sensor_value(id: int):
    """
    :param id: 1:front
               2:right
               3:back
               4:left
    :return: cmd_str
    """
    return "ir_distance_sensor distance " + str(id) + " ?"


def set_led(comp_str: str, r_value: int, g_value: int, b_value: int, effect_str: str):
    """
    :param comp_str: all:所有LED灯
                     top_all:云台所有LED灯
                     top_right:云台右侧LED灯
                     top_left:云台左侧LED灯
                     bottom_all:底盘所有LED灯
                     bottom_front:底盘前侧LED灯
                     bottom_back:所有后侧LED灯
                     bottom_left:所有左侧LED灯
                     bottom_right:所有右侧LED灯
    :param r_value: (int:[0, 255]):RGB 红色分量值
    :param g_value: (int:[0, 255]):RGB 绿色分量值
    :param b_value: (int:[0, 255]):RGB 蓝色分量值
    :param effect_str: solid : 常亮效果
                       off : 熄灭效果
                       pulse : 呼吸效果
                       blink : 闪烁效果
                       scrolling : 跑马灯
    :return: cmd_str
    """
    return "led control comp " + comp_str + " r " + str(r_value) + " g " + \
           str(g_value) + " b " + str(b_value) + " effect " + effect_str


const_xy = 1000 / 3.5
const_z = 1000 * 20 / 600


def set_speed(direct: bool, speed_x: float, speed_y: float, speed_z: float):
    """
    :param direct: 直接设置xyz速度或者转换成麦轮转速
    :param speed_x: (float:[-3.5,3.5]): x 轴向运动速度，单位 m/s
    :param speed_y: (float:[-3.5,3.5]): y 轴向运动速度，单位 m/s
    :param speed_z: (float:[-600,600]): z 轴向旋转速度，单位 °/s
    :return: cmd_str
    """
    if direct:
        return "chassis speed x " + str(speed_x) + " y " + str(speed_y) + " z " + str(speed_z)
    else:
        speed_w1 = int(const_xy * speed_x - const_xy * speed_y - const_z * speed_z)
        speed_w2 = int(const_xy * speed_x + const_xy * speed_y + const_z * speed_z)
        speed_w3 = int(const_xy * speed_x - const_xy * speed_y + const_z * speed_z)
        speed_w4 = int(const_xy * speed_x + const_xy * speed_y - const_z * speed_z)
        print("expect: " + str(speed_x) + " " + str(speed_y) + " " + str(speed_z) + \
              " " + str(speed_w1) + " " + str(speed_w2) + " " + str(speed_w3) + " " + str(speed_w4))
        return "chassis wheel w1 " + str(speed_w1) + " w2 " + str(speed_w2) + \
               " w3 " + str(speed_w3) + " w4 " + str(speed_w4)


def set_wheel(speed_w1: int, speed_w2: int, speed_w3: int, speed_w4: int):
    """
    :param speed_w1: (int:[-1000, 1000]): 右前麦轮速度，单位 rpm
    :param speed_w2: (int:[-1000, 1000]): 左前麦轮速度，单位 rpm
    :param speed_w3: (int:[-1000, 1000]): 右后麦轮速度，单位 rpm
    :param speed_w4: (int:[-1000, 1000]): 左后麦轮速度，单位 rpm
    :return: cmd_str
    """
    return "chassis wheel w1 " + str(speed_w1) + " w2 " + str(speed_w2) + \
           " w3 " + str(speed_w3) + " w4 " + str(speed_w4)


def get_position():
    """
    :return: cmd_str
    """
    return "chassis position ?"


def get_speed():
    """
    :return: cmd_str
    """
    return "chassis speed ?"
